﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace MatlabIG
{
    class MatlabModel
    {
        public double X;
        public double Y;
        public double Z;
        public double[] Mat = new double[9];
        public Matrix PositionMatrix = Matrix.Identity;
        public double[] MotorAngles = new double[2];
        public double[] RPY = new double[3];
        public double[] Velocities = new double[3];

        public void ParseMatlabPacket(byte[] packet)
        {
            X = BitConverter.ToDouble(packet, 0);
            Y = BitConverter.ToDouble(packet, 8);
            Z = BitConverter.ToDouble(packet, 16);
            
            for (int i = 0; i != 9; i++)
            {
                Mat[i] = BitConverter.ToDouble(packet, 24 + i * 8);
            }


            PositionMatrix = Matrix.CreateTranslation((float)X, (float)Y, (float)Z);
            PositionMatrix.M11 = (float)Mat[0];
            PositionMatrix.M21 = (float)Mat[1];
            PositionMatrix.M31 = (float)Mat[2];

            PositionMatrix.M12 = (float)Mat[3];
            PositionMatrix.M22 = (float)Mat[4];
            PositionMatrix.M32 = (float)Mat[5];

            PositionMatrix.M13 = (float)Mat[6];
            PositionMatrix.M23 = (float)Mat[7];
            PositionMatrix.M33 = (float)Mat[8];

            // AUX 1/2
            MotorAngles[0] = BitConverter.ToDouble(packet, 96);
            MotorAngles[1] = BitConverter.ToDouble(packet, 104);

            RPY[0] = BitConverter.ToDouble(packet, 112);
            RPY[1] = BitConverter.ToDouble(packet, 120);
            RPY[2] = BitConverter.ToDouble(packet, 128);

            Velocities[0] = BitConverter.ToDouble(packet, 136);
            Velocities[1] = BitConverter.ToDouble(packet, 144);
            Velocities[2] = BitConverter.ToDouble(packet, 152);
        }

        public string GetPositionString()
        {
            string str;

            str = String.Format("X: {0:0.000} m\nY: {1:0.000} m\nZ: {2:0.000} m\n", X, Y, -Z);
            str += String.Format("{0:0.00} {1:0.00} {2:0.00}\n", Mat[0], Mat[1], Mat[2]);
            str += String.Format("{0:0.00} {1:0.00} {2:0.00}\n", Mat[3], Mat[4], Mat[5]);
            str += String.Format("{0:0.00} {1:0.00} {2:0.00}\n", Mat[6], Mat[7], Mat[8]);
            str += String.Format("Tail[deg]: {0}\n", MotorAngles[0] / Math.PI * 180);
            str += String.Format("R: {0} P: {1} Y: {2}\n", MathHelper.ToDegrees((float)RPY[0]), MathHelper.ToDegrees((float)RPY[1]), MathHelper.ToDegrees((float)RPY[2]));
            return str;
        }
    }
}
